/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* C3DCorrelationScanMatch
* This is a "brute force" scan match algorithm that calculates translation and rotation 
* between two scans. The algorithm is based on calculation of 3D correlation (in pose space)
* 
* The correlation criterion is based loosly on discretation of the scans. Basically the match 
* between scan points is approved if the distance between points is less than specified. The
* correlation value is the sum of hits.
*
* Algorithms that could be reused:
* - Heap sort (hpsrt()) Sorts a float vector and returns the 
*   sorted array and index that corresponds to original index
* - LSQ Pose estimation (MatchPairPoints). Makes a pose estimate 
*   using point pairs.
* 
* @author Jari Saarinen
* @acknoledgements Janne Paanajärvi, Jorma Selkainaho
* 
**/
#ifndef C3DCORRELATION_SCAN_MATCH_H_
#define C3DCORRELATION_SCAN_MATCH_H_
    
    
#include <math.h>
#include "CScanMatchInterface.h"
    
struct Point {
float X,Y;
} typedef TFPoint;


class C3DCorrelationScanMatch : public CScanMatchInterface{
    private:
      /// These parameters form the 3D Grid in which the correlation is calculated
      int NNNX; ///< The number of grid cells in X-coodrdinate direction
      int NNNY; ///< The number of grid cells in Y-coodrdinate direction
      int NNNA; ///< The number of grid cells in Angle estimation
      
      ///These parameters define the size of each grid cell (the resolution)
      float add; ///<Angle resolution (in radians)
      float xdd; ///<X-coordinate resolution [m]
      float ydd; ///<Y-coordinate resolution [m]
      
      /**
       *  The maximum allowed distance between matched closest points 
       * (used in goodness evaluation/as correlation criteria) 
      */
      float ddd;
      
      /**
      * A Least Squares optimisation based on matched pointpairs
      * The pairs should be ordered so that refpoint[i] corresponds to actualpoint[i].
      * @param RefPoints[]    List of points in reference scan
      * @param ActualPoints[] List of points in the current measurement
      * @param N              Number of point pairs
      * @param *Tx            @out Translation in x-dir  
      * @param *Ty            @out Translation in y-dir
      * @param *fi            @out Rotation in heading
      **/
      void MatchPairPoints(TFPoint RefPoints[], TFPoint ActualPoints[], int N,
                           float *Tx, float *Ty, float *fi);
      
      /**
       * Heap Sort algorithm
       * @param *val pointer to values
       * @param *indx The index of values after sort
       * @param N The number of values
       **/
      void hpsrt(float *val,int * indx, int N);
      
      
      /// The number of measurements taken with the laser
      int numOfMeasurements;
      
      
      /// CACHED values
      float *ix, *sinlut, *coslut,*iy,*ix2;
      
      ///////////////////////////////////////////////////////////
      ///////////////////////////////////////////////////////////
      
      /** 
       * Transforms the r,a scan to x,y
       * @param  *r  The rangemeasurements
       * @param *a The angle of measurement
       * @param *x The output x-coordinates
       * @param *y The output y-coordinates
       * @param pose The pose where the r,a scan was taken
       * @param inputSize The size of the scan
       */
      void scan2absoluteXY(float *r, float *a, float *x, float *y, ScanMatch::pose pose, int inputSize);
      /**
       * Integrates the relative movement to new absolute pose
       **/
      ScanMatch::pose integrateNewPositionFromReference(ScanMatch::pose refP, float *v );
      
    protected:
    public:
	/**
	* Default constructor
	* Assumes: 361 measurement with 0.5 deg... 
	*/
	C3DCorrelationScanMatch();
	
        /**
        * My own constructor, this is the only thing that can be different from others
        * TODO documentation!
        */
        C3DCorrelationScanMatch(int NX,
                                int NY,
                                int NA,
                                float xd,
                                float yd,
                                float ad,
                                float dd,
                                int maxNumOfMeasurements);
        
        ~C3DCorrelationScanMatch();
        
        /////////////////////////////////////////////
        //// SCAN MATCH INTERFACE ///////////////////
        ////////////////////////////////////////////
        /**
         * Updates the position estimate with the new measurement @meas. 
         * @param &meas The latest range scan measurement
         * @param &est  The estimated position, where the measurement is from.
         * @param Goodness The goodness (probability/likelihood) of the match [0...1] 1 is perfect 0 is none 
         */
        float update(ScanMatch::scan &meas, ScanMatch::pose est);
	
	
        ///////////////////////////////////////////// /////////////////// ///////////////////
        ////////// C3DCorrelationScanMatch - CANNOT BE USED FROM SCANMATCH INTERFACE ///////
        /////////////////////////////////////////////////////////////// ///////////////////
	/**
	* setNumberOfParticles(int NX,int NY, int NA);
	* 
	* Sets the number of particles used in scan matching
	*
	* NX, number of X grids
	* NY, number of Y grids
	* NA, number of angle grids
	**/
	void setNumberOfParticles(int NX,int NY, int NA);
	
	/**
	* int sickmap(float *a,float *r,float *ix2, float *iy,float *v );
	* 
	* calculates match between reference scan and measured scan
	* 
	* *a - angles corresponding to measurement r
	* *r - measurement
	* *ix2 - x coordinates of reference "map"
	* *iy  - y coordinates of reference "map"
	* *v  - initial position between ref and act scan + the result is returned here
	*  
	* Returns:int hits, number of hits that corresponds to the best match
	**/ 
        int sickmap(ScanMatch::scan &act, ScanMatch::pose est );
	
	/**
	*void setGridSize(float ad, float xd, float yd, float dd);
	*
	* sets the grid size for search grids
	*
	* ad - angle resolution in DEGREES
	* xd - X resolution in meters
	* yd - Y resolution in meters
	* dd - longest distance between closest points... if shorter hit is increased
	**/
	void setGridSize(float ad, float xd, float yd, float dd);
		
};


#endif
